/*
 * CoordinateMath.cpp
 *
 *  Created on: Jan 22, 2011
 *      Author: user
 */


#include "CoordinateMath.h"
#include "math.h"
#include "Constants.h"

//Tested
float normalizeAngle(float theta){
	while (theta >= 360) theta = theta - 360;
	while (theta < 0) theta = theta + 360;
	return theta;
}

//Convert degrees into radians
float r(float degrees){
	return degrees*PI/180.0f;
}

//Convert radians into degrees
float d(float r){
	return r*180.0f/PI;
}

//=============================================OrderedTriple==============================
//Tested
OrderedTriple::OrderedTriple(){
	x=y=z=0;
}
//Tested
OrderedTriple::~OrderedTriple(){

}


//Tested
void OrderedTriple::translateInPlace(float kx,float ky,float kz){
	x += kx;
	y += ky;
	z += kz;
}

//Tested
void OrderedTriple::translateInPlace(Vector& v){
	x += v.getX();
	y += v.getY();
	z += v.getZ();
}
//Tested
float OrderedTriple::getX(){
	return x;
}
//Tested
float OrderedTriple::getY(){
	return y;
}
//Tested
float OrderedTriple::getZ(){
	return z;
}



//======================================== Point3D ================================================

//Tested
Point3D::Point3D(){
	x = y = z = 0;
}
//Tested
Point3D::Point3D(float kx,float ky,float kz){
	x = kx;
	y = ky;
	z = kz;
}
//Tested
Point3D::~Point3D(){

}
//Tested
float Point3D::distanceTo(Point3D& p){
	return sqrt(pow((x-p.getX()),2)+pow((y-p.getY()),2)+pow((z-p.getZ()),2));
}
//Tested
void Point3D::moveTo(Point3D& pt){
	x = pt.getX();
	y = pt.getY();
	z = pt.getZ();
}

void Point3D::moveTo(float xa, float ya, float za){
	x = xa;
	y = ya;
	z = za;
	
}



//======================================== Point3D ================================================


//Tested
Vector::Vector(){
	x = y = z = 0;
}
/* The parameters for vector construction change depending on the first arguement
 * kOrthogonkSphericalCoordinatesalCoordinates: a = x, b= y, c=z
 * kCylindricalCoordinates: a= radius from z axis, b= theta angle from x axis, c = z height
 * kSphericalCoordinates: a= radius from origin, b= theta angle from x axis, c = phi angle from z axis
 */
//tested
Vector::Vector(coordinateType type,float ka,float kb ,float kc){
	switch (type){
		case Vector::kOrthogonalCoordinates:
			x = ka;
			y = kb;
			z = kc;
			break;
		case Vector::kCylindricalCoordinates:
			x = ka*cos(r(kb));
			y = ka*sin(r(kb));
			z = kc;
			break;
		case Vector::kSphericalCoordinates:
			x = ka*cos(r(kb))*sin(r(kc));
			y = ka*sin(r(kb))*sin(r(kc));
			z = ka*cos(r(kc));
			break;
		default:
			x = y = z = 0;
			break;
	}
}

//
Vector::Vector( Point3D &from, Point3D &to){
	x = to.getX() - from.getX();
	y = to.getY() - from.getY();
	z = to.getZ() - from.getZ();
}


// Tested
Vector::~Vector(){

}

//
float Vector::getR(coordinateType type){
	switch (type){
		case Vector::kCylindricalCoordinates:
			return sqrt(pow(x,2)+pow(y,2));
		case Vector::kSphericalCoordinates:
			return sqrt( pow(x,2) + pow(y,2) + pow(z,2));
		case Vector::kOrthogonalCoordinates:
			return 0;
	}
}

//
float Vector::getTheta(){
	return d(atan2((double)y,(double)x));
}

//
float Vector::getPhi(){
	return d(atan2((double)sqrt(pow(x,2)+pow(y,2)),(double)z));
}


